#include <utils/bitmap.h>

#include <utils/memutils.h>

void bitmap_set(bitmap_t *bitmap, size_t index, size_t count, bool value) {
	if(index + count > bitmap->size || count == 0) {
		return;
	}

	uint8_t first = index & 7;
	uint8_t first_mask = 0xff << first;
	uint8_t *first_byte = &bitmap->data[index >> 3];

	if(first == 0) {
		goto skipfirst;
	}

	if(count <= 8ul - first) {
		uint8_t shift = 8ul - first - count;
		first_mask = ((first_mask << shift) & 0xff) >> shift;
		count = 0;
	} else {
		count -= 8ul - first;
		index += 8ul - first;
	}

	if(value) {
		*first_byte |= first_mask;
	} else {
		*first_byte &= ~first_mask;
	}

skipfirst:
	uint64_t middle = count >> 3;
	uint8_t middle_mask = value ? 0xff : 0;

	memset(&bitmap->data[index >> 3], middle_mask, middle);
	index += middle << 3;

	count &= 7;

	uint8_t last = count;
	uint8_t last_mask = 0xff >> (8 - last);
	uint8_t *last_byte = &bitmap->data[index >> 3];

	if(value) {
		*last_byte |= last_mask;
	} else {
		*last_byte &= ~last_mask;
	}
}

bool bitmap_is_any_set_as(bitmap_t *bitmap, size_t index, size_t count, bool value) {
	if(index + count > bitmap->size || count == 0) {
		return false;
	}

	uint8_t first = index & 7;
	uint8_t first_mask = 0xff << first;
	uint8_t first_byte = bitmap->data[index >> 3];

	if(first == 0) {
		goto skipfirst;
	}

	if(count <= 8ul - first) {
		uint8_t shift = 8ul - first - count;
		first_mask = ((first_mask << shift) & 0xff) >> shift;
		count = 0;
	} else {
		count -= 8ul - first;
		index += 8ul - first;
	}

	if((first_byte & first_mask) != (value ? 0 : first_mask)) {
		return true;
	}

skipfirst:
	uint64_t middle = count >> 3;
	uint8_t middle_mask = value ? 0 : 0xff;

	for(uint64_t i = 0; i < middle; ++i, index += 8) {
		if(bitmap->data[index >> 3] != middle_mask) {
			return true;
		}
	}

	count &= 7;

	uint8_t last = count;
	uint8_t last_mask = 0xff >> (8 - last);
	uint8_t last_byte = bitmap->data[index >> 3];

	return (last_byte & last_mask) != (value ? 0 : last_mask);
}

bool bitmap_is_all_set_as(bitmap_t *bitmap, size_t index, size_t count, bool value) {
	if(index + count > bitmap->size || count == 0) {
		return false;
	}

	uint8_t first = index & 7;
	uint8_t first_mask = 0xff << first;
	uint8_t first_byte = bitmap->data[index >> 3];

	if(first == 0) {
		goto skipfirst;
	}

	if(count <= 8ul - first) {
		uint8_t shift = 8ul - first - count;
		first_mask = ((first_mask << shift) & 0xff) >> shift;
		count = 0;
	} else {
		count -= 8ul - first;
		index += 8ul - first;
	}

	if((first_byte & first_mask) != (value ? first_mask : 0)) {
		return false;
	}

skipfirst:
	uint64_t middle = count >> 3;
	uint8_t middle_mask = value ? 0xff : 0;

	for(uint64_t i = 0; i < middle; ++i, index += 8) {
		if(bitmap->data[index >> 3] != middle_mask) {
			return false;
		}
	}

	count &= 7;

	uint8_t last = count;
	uint8_t last_mask = 0xff >> (8 - last);
	uint8_t last_byte = bitmap->data[index >> 3];

	return (last_byte & last_mask) == (value ? last_mask : 0);
}
